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融合 IMU 去 除 运 动 模糊 的 改进 光 流 匹配 算法 “ 
栾 小 珍 *:， 魏 国 亮 "， 荣 ghe 
(上 海 理 工大 学 a. 理学 院 ; b. 管理 学 院 ， 上海 200093) 


摘 要 :为 进一步 提高 视觉 SLAM 中 的 光 流 匹配 精度 和 速度 ,提出 一 种 融合 惯性 测量 单元 (Inertial Measurement Unit, 

IMU) 去 除 运动 模糊 的 改进 光 流 匹配 算法 。 该 算法 首先 利用 MU 运动 信息 计算 的 点 扩散 函数 去 除 运 动 模糊 ， 提 高 特 
征 点 匹配 率 ; 其 次 在 LK(Lucas-Kanade) 光 流 的 基础 上 ， 引 入 梯度 误差 ， 并 使 用 图 像 梯度 L1 范 数 作为 正则 项 模拟 稀 
RRA, WERB: 然后 利用 IMU 预测 特征 点 位 置 作为 该 算法 初始 值 ， 并 加 入 BB(Barzilar-Borwein) 步 长 改进 
原 有 的 高 斯 牛顿 算法 ， 提 高 计算 速度 。 实 验 表明 ， 通 过 两 帧 之 间 比 较 ， 该 算法 的 效率 和 精度 均 优 于 LK 光 流 法 ; 然 
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Feature tracking algorithm for removing motion blur by improving optical flow incorporating IMU 


Luan Xiaozhen?, Wei Guoliang^!, Cai Jie? 
(a. College of Science, b. Business School, University of Shanghai for Science & Technology, Shanghai 200093, China) 


Abstract: In order to improve the accuracy and efficiency of the feature point matching, this paper proposed a novel feature 
point matching algorithm in terms of vision and inertial measurement unit (IMU) fusion. Firstly, the algorithm calculated the 
point diffusion function using the motion information of IMU to remove motion blur, and improved the feature point matching 
rate. Secondly, based on LK (Lucas-Kanade) optical flow method, this paper introduced gradient error and uses L1 parametric 
to simulate sparse noise. Furthermore, the feature point position by using IMU is the initial value of the algorithm, and then 
this paper used BB (Barzilar-Borwein) step to improve the efficiency of the algorithm. Finally, the comparison experiments 
show that the efficiency and accuracy of the algorithm are better than the LK optical flow method. Especially, the algorithm 
improves the localization accuracy and robustness of the VINS-Mono framework on the dataset EuRoC. 
Key words: optical flow; visual odometry; motion blur; multi-sensor fusion 
0 引言 在 光 流 法 中 前 主要 存在 以 下 两 个 问题 ， 一 方面 ， TH 
体 运动 过 快 ， 则 在 相机 曝光 期 间 ， 景 物 和 摄像 机 之 间 存 在 相 
近年 来 ， 视 觉 同 时 定位 与 建 图 (Visual Simultaneous ”对 运动 ， 会 导致 像素 重 骆 ， 出 现 图 像 模 糊 现 象 ， 称 为 运动 模 
Localization And Mapping，V-SLAMI)D 在 机 器 人 领域 有 着 糊 。 而 光 流 法 对 光度 稳定 和 图 像 的 连续 有 较 强 依赖 ， 当 不 满 
要 的 作用 ,如 物流 配送 、 虚 拟 现实 (Virtual Reality，VR)JP、 智 。 是 以 上 假设 的 时 候 ， 容 易 找 不 到 跟踪 点 ， 更 糟糕 的 情况 是 追 
能 家 居 等 。 虽 然 单 目 相 机 成 本 较 低 、 使 用 方便 ,但 是 仅仅 依 。 踪 到 错误 的 特征 点 。 另 一 方面 ， 在 光 流 法 中 使 用 非 线 性 优化 
靠 相 机 ， 难 以 估计 环境 的 尺度 。 因 此 在 V-SLAM F, 为 了 解 ” 方法 进行 最 小 化 光度 误差 迭代 匹配 的 过 程 中 ， 首 先 ， 非 线性 
决 尺度 不 可 观 问 题 ， 众 多 学 者 更 倾向 于 多 传感器 融合 技术 。 算法 的 最 优 值 对 初 值 有 很 强 的 依赖 性 ， 如 果 没 有 初 值 则 需要 
由 于 惯性 测量 单元 (Inertial Measurement Unit, IMU)BE E [fij 进行 全 局 搜索 ， 计 算 量 较 大 ， 且 容易 造成 误 匹 配 ， 其 次 ， 传 
绝 外 部 环境 的 影响 下 测量 本 体 的 角速度 和 加 速度 ， 从 而 使 单 。” 统 高 斯 牛顿 方法 的 步 长 是 固定 步 长 或 者 直接 由 线 搜索 算法 给 
目 视觉 的 公制 尺度 可 观测 ， 并 提供 鲁 棒 和 准确 的 帧 间 运 动 估 ”出 ， 如 果 步 长 取 的 太 大 ， 一 开始 会 很 快 向 最 优 解 到 近 ， 在 收 
计 ， 因 此 视觉 和 IMU 融合 的 SLAM 技术 (VI-SLAM) 寞 军 突 。 人 钱 的 最 后 阶段 ， 容 易 跳 过 最 优 解 ， 而 步 长 取 的 太 小 ， 则 导致 
起 B51。VI-SLAM 框架 主要 分 为 前 端 里 程 计 、 后 端 优化 、 丐 收敛 速度 变 慢 。 因 此 ， 去 除 运动 模糊 和 选取 非 线 性 优化 算法 
环 检测 和 建 图 。 前 端 里 程 计 作为 首要 步骤 ， 在 整个 系统 中 显 。 的 初始 值 、 步 长 是 本 文 研究 的 两 个 问题 。 
得 尤其 重要 。 在 前 端 里 程 计 中 ， 不 仅 需 要 进行 IMU 预 积 分 ， 基于 以 上 问题 , 本 文 提出 了 一 种 融合 IMU 去 除 运动 模糊 
而 且 需 要 对 相机 传 入 的 图 片 进行 特征 点 提取 和 匹配 ， 进 而 根 ”的 光 流 匹配 算法 。 该 算法 通过 融合 IMU 运动 信息 , 计算 点 扩 
据 匹 配 信息 计算 相 邻 两 帧 之 间 机 器 人 的 运动 ， 并 构建 特征 点 。 散 函 数 去 除 运 动 模糊 , 之 后 再 次 融合 IMU 信息 预测 特征 点 位 
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的 局 部 3D 位 置 外 。 一 旦 发 生 误 匹配 仆 ， 会 导致 估计 不 准确 ， ” 置 ， 为 光 流 跟踪 提供 初 值 ， 进 而 提高 特征 点 提取 与 匹配 的 性 
因此 ， 特 征 点 提取 与 匹配 也 是 前 端 里 程 计 中 的 重要 一 环 。 能 。 最 后 ， 将 该 算法 集成 到 了 完整 VI-SLAM 框架 中 。 
特征 点 提取 与 匹配 主要 分 为 特征 点 法 8 外 、 光 流 法 LU 和 直 在 VI-SLAM 框架 特征 点 提取 与 检测 方面 ，Tong Qin 等 
接 法 上 0。 特征 点 法 在 计算 关键 点 、 描 述 子 ， 以 及 匹配 描述 子 时 是 出 的 VINS-Mono02] 前 端 使 用 光 流 法 提取 关键 点 , 并 使 用 多 
计算 量 过 大 ， 而 光 流 法 和 直接 法 弥补 了 以 上 缺陷 ， 光 流 法 只 计 层 金 字 塔 LK(Lucas-Kanade) 进 行 跟踪 ， 虽 然 整体 框架 是 基于 
算 关键 点 ， 不 计算 描述 子 ， 通 过 两 帧 之 间 的 光度 不 变 假设 ， 直 IMU 和 视觉 传感器 融合 ， 但 在 关键 点 提取 和 跟踪 中 ， 并 未 将 
接 最 小 化 光度 误差 并 进行 迭代 匹配 ， 本 文 重 点 研究 光 流 法 。 IMU 信息 与 视觉 信息 融合 。 Qi Guan 等 提出 的 IFPT 特征 点 匹 
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配 算法 将 特征 点 法 与 IMU 进行 融合 031， 通 过 IMU 的 位 置 和 
旋转 方向 预测 下 一 帧 关键 点 的 位 置 ， 并 以 其 为 圆心 ， 进 行 撒 
述 子 局 部 搜索 与 匹配 。OKVIS(Open Keyframe-based Visual- 
Inertial SLAM)U4Iff] 3D-2D 的 匹配 中 ， 利 用 IMU 预测 的 位 姿 
第 选 当 前 帧 可 见 的 特征 点 ， 去 除 掉 当 前 帧 已 经 追踪 不 到 的 特 
征 点 ， 节 省 暴力 匹配 时 间 。OKVIS 和 IFPT 共同 点 是 将 特征 
点 法 与 IMU 融合 , 即 基于 预 积 分 理论 进行 预测 ,为 待 匹 配 的 
每 个 特征 点 提供 新 图 像 中 的 可 能 位 置 ， 进 而 完成 前 端 特 征 点 
提取 与 匹配 。 不 同 点 是 FPT 得 到 可 能 位 置 之 后 进行 局 部 搜 
索 , 而 OKVIS 筛选 去 除 当 前 帧 已 经 看 不 到 的 点 , 然后 进行 全 
局 暴力 匹配 。 
本 文 提出 的 算法 和 之 前 相关 工作 有 所 区 别 ， 首 先 从 算法 
本 身 比较 : a) 之 前 的 工作 [15] 直 接 设 置 平移 信息 +=0 ， 而 提 
出 的 算法 充分 利用 了 IMU 的 信息 ,加 入 了 加 速度 计 得 到 的 7， 

实验 结果 表明 融合 平移 信息 后 ，IMU 预测 的 特征 点 位 置 更 加 
准确 ; b) 之 前 的 工作 未 将 速度 信息 充分 利用 , 而 提出 的 算法 
利用 IMU 预 积分 得 到 的 运动 信息 , 计算 点 扩散 函数 ,加 入 了 

运动 模糊 复原 步骤 ,减少 运动 模糊 导致 匹配 特征 点 失败 现象 ; 

c) 在 非 线 性 优化 方法 中 ,加 入 BB(Barzilar-Borwein) 可 变 步 长 
进行 搜索 ， 使 步 长 大 小 可 以 自 适 应 损失 函数 。 其 次 ， 从 框架 
上 比较 ， 之 前 的 工作 05 把 偏 置 当 作 状态 向 量 的 一 部 分 去 优化 ， 
并 且 在 初始 化 前 ， 偏 置 求 解 不 准确 的 情况 下 ， 仍 然 预测 相机 
位 姿 ， 导 致 视觉 里 程 计 的 误差 一 直 传递 下 去 ， 更 糟糕 的 是 若 
系统 中 没有 闭环 检测 ,很 难 消除 视觉 里 程 计 产生 的 累计 误差 。 

本 文 提出 的 算法 利用 IMU 预测 的 时 机 是 在 系统 初始 化 后 , 此 
时 偏 置 信息 已 经 求 出 ， 减 小 了 累计 误差 。 


1 ”系统 框架 


该 算法 在 VINS-Mono 框架 前 端 算法 基础 上 进行 研究 。 
原 VINS-Mono 框架 前 端 算法 使 用 ShiTomasi0617 算 法 检测 角 
点 , LK 光 流 法 跟踪 匹配 。 在 运动 幅度 较 大 ， 光 照 变化 剧烈 情 
况 下 ， 匹 配 误 差 较 大 。 
为 解决 以 上 问题 , 提高 定位 精度 , 本 文 对 VINS-Mono 前 
端 算法 进行 改进 。 有 具体 实施 方案 如 下 : 首先 在 初始 化 前 ， 仍 
通过 传统 光 流 法 进行 特征 点 提取 ， 初 始 化 完成 之 后 ， 将 得 到 
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其 中 :ww 为 陀螺 仪 在 IMU 坐标 系 下 真实 的 角速度 值 ，”%w 
为 陀螺 仪 在 IMU 坐标 系 下 测量 得 到 的 角速度 值 ，“e" 为 加 速 
度 计 在 IMU 坐标 系 下 真实 的 加 速度 值 ， “ov 为 加 速度 计 在 
IMU 坐标 系 下 测量 得 到 的 加 速度 值 。 闪 ， 久 的 导数 服从 高 斯 
分 布 : 


b: ~ NO, op), bs ~ NO. o: 


其 中 : nj, ni 分 别 为 陀螺 仪 、 加 速度 计 偏 置 的 随机 游 走 噪声 ， 
服从 高 斯 分 布 : 
ns ~ NO, o), n? ~ NO, o, 
前 端 
D 传统 特征 点 提取 
3082) po 
mn nemit = MEME! C a MVEK PA 
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对 图 像 块 进行 卷 积 去 
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基于 BB 可 变 步 长 的 高 初始 值 由 IJ 预 积分 将 到 在 图 像 决 内 进行 最 小 光度 
BUSES, WHITES A PORNE < 中 一 一 一 ”误差 + 梯度 误差 + 像素 一 范 
ng 位 置 数 正则 项 


后 端 l 回环 检测 


下 
RINORE — — — r— 是 否 是 关键 外? — 回环 检测 


回 坏 检测 结果 


图 1 系统 框架 

Fig. 1 System framework 
在 相机 坐标 系 下 ， 分 别 将 7 时刻 机 器 人 人 位置、 速度、 旋 
转 方向 对 时 间 分 别 求 导 得 : 


Por = Voy; 


由 移动 的 
dx, dy, Y 计 算 模 
MAT 


Voy; maei (2) 


. 0 
doy, — doy, 四 ly, 
2 "a, 


的 IMU 加 速度 计 和 角速度 计 偏 置 代 入 到 预 积分 中 , 融合 预 积 
分 预测 得 到 的 速度 、 位 置 、 旋 转 方向 信息 ， 发 送 到 前 端 ， 判 
断 速度 是 否 大 于 阔 值 ， 如 果 大 于 阔 值 说 明和 运动 过 快 ， 则 根据 
运动 信息 进一步 对 图 像 进行 非 盲 复原 0%! 半 。 如 果 上 一 步 完 成 
或 者 速度 小 于 闵 值 ， 说 明 运动 幅度 较 小 ， 为 满足 实时 性 ， 则 不 
进行 图 像 复 原 操作 。 直 接 将 IMU 预测 得 到 的 位 置 、 旋 转 方向 与 
视觉 信息 融合 ， 即 将 恢复 之 后 的 点 投影 到 下 一 帧 图 像 上 ， 提 供 
给 光 流 追踪 中 高 斯 牛顿 算法 一 个 初始 值 ， 确 定 光 流 追 踪 匹 配 的 


局 部 搜索 位 置 。 该 算法 集成 到 框架 上 ， 其 流程 如 图 1 所 示 。 
2 ” 预 积 分 模型 预测 特征 点 位 置 
本 节 首先 对 常用 到 的 一 些 符号 进行 定义 。() 表示 传感器 


其 中 ， Pos, 为 从 相机 第 一 帧 坐标 系 到 IMU 第 i 帧 坐标 系 的 平 
移 向 量 ; vo 为 相机 坐标 系 下 ， 7 时 刻 机 器 人 速度 , 用 三 维 向 
量 表 示 ; 4o 为 从 IMU 第 i 帧 坐标 系 到 相机 第 一 帧 坐标 系 的 
旋转 和 矩阵， 用 四 元 数 表 示 。m 、w, 、w 表示 将 机 器 人 的 位 
置 、 速 度 、 旋 转 方向 分 别 对 时 间 求 导 。 


对 式 (2) 进 行 积分 得 到 在 相机 坐标 系 下 ，j 时 刻 机 器 人 的 
位 置 、 速 度 、 旋 转 方 向 : 
Per = Peg, Voy; Ab gan 
Vau, = Voy, , + At (3) 


1 
Iesi; 7 deus sh | 
gj A 


测量 值 ，& 表示 3x3 的 旋转 矩阵 ,4 表示 旋转 的 四 元 数 表 示 ， 

1 表示 3 维 平移 向 量 ，7T=[R| 表示 姿态 的 转换 和 矩阵， 包括 旋 
转 和 矩阵 和 平移 向 量 。0), 为 IMU BRR, Oc 为 相机 坐标 系 ， 
特别 地 ， 令 相机 的 第 一 帧 为 参考 坐标 系 ， 即 旋转 为 单位 矩阵 
Re =E ,平移 向 量 为 零 向 量 t,=0 。 假 设 经 过 初始 化 P020 之 后 ， 
从 相机 坐标 系 到 IMU 坐标 系 的 旋转 外 参 RS, FHINE toe, 
IMU 陀螺 仪 的 偏 置 vz ,加 速度 计 偏 置 ,以 及 在 参考 帧 坐标 


其 中 Ac 为 ;时 刻 与 了 7 时刻 的 时 间 间 隔 ， 采 用 中 值 法 得 到 7 时 
刻 的 加 速度 4 和 角速度 w s 


1 
a= 4er, , (dco, UP) dg + doy, (acy, -b*)- sc | 


1 (4) 
wy [ara b)+ War 7 br) 
但 是 在 优化 位 姿 过 程 中 ， 当 前 帧 的 速度 和 旋转 矩阵 等 会 发 
生 改 变 , 需要 重新 进行 积分 , 计算 量 较 大 , 故 引入 预 积分 请 ”1 


I 


系 下 的 重力 矢量 86 是 已 知 的 。 

2.1 基于 预 积分 求解 机 器 人 的 位 置 、 速 度 、 旋 转 方向 

只 考虑 陀螺 仪 和 加 速度 计 的 偏 置 白 噪声 以 及 偏 置 的 随机 
游 走 噪声 ， 则 IMU 模型 为 232; 


cy, = dc, 8 4ni, (5) 


其 中 ，@ 表示 四 元 数 乘法 [22]， 将 (5) 代 入 (4) 更 新 加 速度 : 


1 
a — Qc, e tus. (aq, , -b")« di, (acy, =b" )] =g 


:202205.00070v1 


chinaXiv 


录用 定稿 


引入 预 积 分 之 后 运动 学 模型 的 离散 形式 : 


1 
Por = Po veo At F380 + day, & Q, 


6 
Van 7 Vor 一 Sc, At + day, e Pu, ( ) 
deu; 7 deus Gy, 
其 中 : 
1 2 
Qu = Slan ; Gu, Cb) ani, cur, -b*)]Ar 
Bu, = > ari, (lacs 7 P7) + qiu, lacy, =°) At 
2 


1 


Yn, =| 1 
m z ana 7 B5) + (Wau, - b) | 


Ar 量 ， 计 算式 (6) 得 到 了 相机 坐标 系 下 机 器 人 的 位 


度 、 旋 转 方向 。 
2.2 二 
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y= xa] x=0, 1, ...,h—l 
hæ ^ (15) 
regn y=0, 1, ...,b—1 


根据 点 扩散 函数 建立 模糊 复原 模型 fos») P173 


f(x,y)= ag min ||g (x, y)- fa y) 8 h(x, y)]; (16) 


Hcr lige» - £05) 8 ^o. > 为 图 像 保 真 项 ， 保 证 清晰 图 像 经 

过 点 扩散 函数 作用 后 , 与 模糊 图 像 一 致 。 式 (16) 可 通过 快速 傅 

Hip 2E GR REUS), 

3.2 改进 原 有 光 流 法 优化 求解 匹配 特征 点 
本 文 在 原 有 LK 光 流 假设 两 帧 图 像 之 间 同 一 特征 点 光度 

不 变 的 基础 上 ， 首 先 ， 加 入 梯度 不 变 假设 : 

VI (x, y) = V1,(x+Ax,Ay), 


充分 利用 特征 点 与 相 邻 像素 的 信息 , 保证 正确 的 匹配 对 。 


结合 2.1 节 得 到 的 结果 计算 相机 坐标 系 下 :和 > 两 帧 2 
间 的 转换 矩阵 Ze : 


Toc, Tec =Tcc (8) 
其 中 

Tac, = Tor Trc? Tac, =Teu Trc, (9) 
将 式 (9) 代 入 式 (8) 化 简 分 别 得 到 i 和 j DL E e PER ER 

Rec 和 平移 向 量 EB te, Gs 
Rec = Re Ron, Ron, Ric 
lec = Rī [Rey (Re tic t tay, —fey, )-ticl (10) 
其 中 ,Rc 为 从 IMU 华 标 系 到 相机 坐标 系 的 旋转 矩阵 ; Ron 
为 从 相机 第 一 帧 坐标 系 到 IMU 第 j 帧 坐标 系 的 旋转 矩阵 ; 
Rwy， 为 从 IMU 第 i 帧 坐标 系 到 相机 第 一 帧 坐标 系 的 旋转 甜 
EE; Re 为 从 相机 坐标 系 到 IMU 坐标 系 的 旋转 矩阵 ， hc 为 
从 IMU 坐标 系 到 相机 坐标 系 的 平移 向 量 ;， t, 为 从 相机 第 一 
帧 坐标 系 到 IMU 第 i 帧 坐标 系 的 平移 向 
将 第 i 帧 的 特征 点 归 一 化 坐标 p; ， 映 射 到 第 j 帧 。 得 到 


四 | 
lm LL 
: 


其 次 ,为 了 防止 过 拟 合 ,使 用 第 二 帧 图 像 梯 度 Li 范 数 模拟 稀 
朴 噪声， 构建 正则 化 项 ， 最 终 模 型 建立 如 下 : 


(Ax, Ay)= arg min(|, (x, y) — Z, (x + Ax, Ay)||, + 
(Ax, Ay) 


a [V Go y) 7 VL (x + Ax, AV) + 2 |V Œ, ll ga 
其 中 : wy) 表示 参考 帧 在 像素 坐标 y) 处 图 像 的 灰 度 值 ; 
(x+Ax,Ay) 表示 当前 帧 在 像素 坐标 (x+Ax,Ay) 处 图 像 的 灰 度 
值 ; Vix,y) 表示 参考 帧 在 像素 坐标 Qe y) 处 图 像 的 梯度 值 ; 
VI (x+ Ax, Ay) 表示 当前 帧 在 像素 坐标 Qc Ax, Ay) 处 图 像 的 梯度 
值 ; < 为 梯度 权重 系数 ， 6 为 稀疏 噪声 项 权重 系数 。 
在 2.2 节 中 , 已 经 通过 IMU 预测 得 到 第 二 帧 特征 点 的 位 
置 ， 将 预测 位 置 作 为 高 斯 牛顿 非 线性 优化 算法 初始 值 ， 并 在 
原来 固定 步 长 基础 上 加 入 BB 可 变 步 长 进行 迭代 。 
将 式 (17) 非 线性 函数 F(X) 进行 泰勒 展开 至 二 阶 : 


FQX)-FOX,) € VECX,40CX — X4) 


18 
KX) Hs X- X) ( ) 


EP, x 为 优化 变量 ，Xi 为 优化 变量 在 第 步 的 取 值 ，F(X) 


p, = Roc Pit toe (11) 


其 中 为 p; 的 齐 次 坐标 ， 大 小 为 3x1 的 向 量 ，2; 为 IMU 预测 
得 到 的 特征 点 在 第 j 帧 下 的 归 一 化 坐标 。 


3 IMU 去 除 运动 模糊 光 流 特征 点 跟踪 算法 


运动 模糊 图 像 的 复原 模型 可 以 描述 为 一 个 清晰 图 像 经 过 
退化 函数 作用 之 后 , 再 加 上 一 个 噪声 项 。 设 一 幅 清 晰 图 像 S Qe y) ， 
经 过 退化 函数 卷 积 ， 则 产生 的 退化 图 像 8(x,y) ， 具 体 为 

g(x.y) 2 f GG y)*hGo y) nGs y) (12) 
其 中 a(x y) 称 为 模糊 算 子 或 点 扩散 函数 ，* 表示 卷 积 ，n(x,y) 
为 加 性 噪声 。 


由 于 通过 IMU 得 到 的 加 速度 和 角速度 可 以 确定 运动 信息 ， 
此 研究 重点 放 在 非 盲 复原 模型 。 
3.1 融合 IMU 先 验 信息 的 运动 模糊 非 盲 复原 模型 

当 两 幅 图 像 的 时 间 戳 ， 即 时 间 间 隔 必 已 知 时 ， 通 过 (7) 得 
到 了 机 器 人 在 两 帧 之 间 的 速度 信息 ， 进 而 利用 (11) 计 算 了 特 
征 点 在 两 帧 的 归 一 化 坐标 Qu v) 、(w, v) ,于 是 得 到 机 器 人 运 
动 模糊 方向 的 角度 和 长 度 : 


0 = arctan( ——) 
ud u, (13) 


L-vxt; 


其 中 ”表示 机 器 人 速度 , t 为 时 间 间 隔 。 通 过 运动 模糊 的 方向 9 


和 长 度 L ， 则 可 确定 运动 模糊 的 点 扩散 函数 宽度 2 、 高 度 h: 
b-LxcosQ0 
seu e9 
点 扩散 函数 hox, y) 由 以 下 隐 函 数 表 示 为 


复原 模型 可 以 根据 有 无 运动 先 验 分 为 盲 复 原 和 非 盲 复原 。 


为 非 线 性 函数 F(X) 在 Xi 处 的 取 值 ，VF(Xi) 为 非 线性 函数 
F(X) 在 Xi 处 的 梯度 值 ，Hx 为 F(X) 在 Xi 处 的 海 塞 矩阵 。 


对 式 (18) 求 导 ， 令 其 为 0， 得 优化 变量 XY=X -HVX , 
则 优化 方向 d. 为 
d, - -H3VF(X,) (19) 
下 面 对 步 长 进行 求解 ， 对 于 一 般 地 二 次 函数 优化 问题 
FQO- 5 X'AX 4 brX (20) 
其 中 4 表示 二 次 函数 的 系数 矩阵 ，2 为 偏 置 。 对 式 (20) 求 导 ， 
得 一 阶 导 函数 F(X)=AX+b ， 则 第 k 步 迭代 梯度 为 
VF(X,) - AX, +b Q1) 


将 式 (21) 代 入 式 (19), WMA H eg Ur HEURES 8:， 近 


似 迭 代 方 向 : 
d,  CHUVF(X,) &-g,l VF(X,) = 一 87 VF(X,) (22) 
其 中 /为 单位 矩阵 ， 定 义 自 变量 的 差 5.，， 梯 度 差 Yu: 
Ska = X, -Xr Ya 2 VF(X,)- VF(X44) (23) 
对 式 (21) 在 第 与 第 k-1 处 做 差 : 
VF(X.)—VF(X,)= AX, 一 4X = Aspa (24) 
则 得 到 拟 牛 顿 方程 : 
AS, , = (gi DS, =Y (25) 
tX QS) oot, UB pb es 
gj zaemi ;ls. -Yal Q6) 
求 得 式 (26) 的 最 小 二 乘 解 : 
SEIS 
Bk EY. Q7) 
式 (27) 可 以 看 出 ， gi 仅 与 第 上 步 自 变量 状态 信息 和 相 


函数 的 梯度 有 关 ， 与 其 他 变量 无 关 。 


来 小 珍 ， 


4 ”实验 结果 与 分 析 


等 : 融合 IMU 去 除 运动 模糊 的 改进 光 流 匹配 算法 
二 乘 误 差 最 小 解 : 


size 
all. error = > error (i) W^'error (i) 
i=l 


本 文 所 有 实验 均 在 同一 平台 进行 ,平台 主要 配置 为 AMD 


Ryzen 5 3500U with Radeon Vega Gfx 


| 核 笔记 本 处 


里 器 ， 内 


存 为 12GB， 基 ; 


的 算法 与 VINS-Mono 框架 中 匹配 算法 


Ex 2.10GHz, Ubuntu18.04 
在 本 节 ， 首 先 单 独 对 算法 本 身 的 性 能 进行 
E LK 光 流 法 、ORB 特 


操作 系统 。 
右 ， 将 提出 


N 


7 


度 。 其 次 ,为 了 进一步 验证 提出 的 优化 算法 的 准 
系统 框架 中 的 稳定 性 ， 将 该 算法 集成 到 


48, 与 原 VINS-Mono. PL-VIOP?!, 


征 点 检测 算法 对 比 ， 测 试 在 两 帧 之 间 匹 配 运行 


VINS-FusionP6l e$. 


7 时 间 和 匹配 精 
E 确 性 以 及 在 


VINS-Mono 系统 框 


相机 


加 IMU(Mono-IMU), VINS-Fusion 双 目 相机 加 IMU(Stereo- 


IMU) 进 行 对 比 ， 评 价 定位 精度 。 
实验 选 
Vehicle MAV) E 


ACH ETUR 49 PE cH 


间 场 景 。 为 了 验证 所 提出 算法 


该 算法 使 用 了 3 个 场景 不 同 难 度 的 9 个 序列 : MH 02 easy, 
MH 03 medium, MH 04 difficult, V1 01 easy, V1 02 


的 数据 集 是 机 器 人 微型 飞行 器 (Micro Aerial 
数据 集 EuRoC， 飞 行 场景 
是 苏黎世 联邦 理工 学 院 ETH 的 一 个 机 器 工厂 和 两 个 普通 房 
和 普通 场景 数据 下 的 有 效 性 ， 


medium, VI 03 difficult, V2 01 easy, V2 02 medium, 


V2 03 difficult. 
4.1 两 帧 之 间 算 法 运行 时 间 对 比 


本 文 提出 的 算法 与 LK 光 流 法 同属 于 光 流 法 ， 在 比较 匹 


配 的 运行 时 间 时 ， 实 验 控制 特征 点 
ShiTomasi 算法 的 角 点 检测 ， 利 


是 取 算 法 相同 ， 均 采用 
] OpenCV 中 


cv::goodFeaturesToTrack， 其 中 提取 算法 参数 : 
500， 质 量 水 平 系数 0.01， 最 小 距离 30， 其 


最 大 角 点 数 


H. rH error(i) 为 观测 数 志 
小 时 ， 两 帧 之 间 


WB. WOW ER 
匹配 程度 也 越 高 ， 定 义 匹配 分 数 : 
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score — 5a / (error (i)+1))" W^ (1/ (error (i)+1)) 
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(29) 


E 阵 ， 当 最 小 二 乘 解 越 


(30) 


E 点 法 检测 加 匹配 时 间 对 比 


Fig.3 Comparison of detection and matching time between 


根据 以 上 定义 的 记 
本 文 提出 的 算法 精度 


the proposed algorithm and ORB method 


PE 估 标准 ,计算 


< 


n WEE 


于 提出 的 改进 


"E RUE 28005 


比 光 流 法 精度 提高 J 
沁 化 模型 (17)， 即 在 原 有 光度 不 变 假设 的 基础 
上 ， 引 入 梯度 误差 和 噪声 稀疏 项 。 这 一 对 
改进 优化 模型 的 合理 ' 


他 参数 都 采用 


cV::goodFeaturesToTrack 默认 参数 。 将 算法 循环 1000 次 ， 
平均 值 ， 提 出 的 算法 平均 运行 时 间 为 : 1.361 毫秒 ; 


中 LK 光 流 法 : 2.892 毫秒 , 速度 提高 52.93996 


取 


， 随 即 取 20 次 


实验 结果 如 图 2 所 示 。 
当 所 提出 的 算法 和 特征 点 法 比较 的 时 候 ， 由 于 特征 点 法 
利用 描述 子 进行 匹配 ， 所 以 在 比较 的 过 程 中 ， 需 要 加 入 计算 


描述 子 的 时 间 ， 另 外 ， 


描述 子 与 特征 点 检测 算法 往往 是 一 一 


对 应 的 ， 所 以 最 终 的 比较 策 


的 算法 平均 运行 时 间 为 : 8.382 毫秒; 
点 法 特征 点 提取 与 匹配 
76.125%， 结 果 如 图 3 所 示 。 
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| 
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—8— cv:calcOpticalFlowPyrLK 
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ja 


至 
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20 
实验 随机 抽样 


图 2 提出 的 算法 与 LK 光 流 法 匹配 时 间 对 比 


各 是 将 特征 点 提取 时 间 加 匹配 时 
间 一 起 进行 比较 。 同 样 将 算法 循环 1000 次 ， 取 平均 值 ， 提 出 
OpenCV 中 ORB 特征 
FE 均 时 间 为 :35.110 毫秒 ， 速 度 提 高 


] 0.010 
4 0. 008 
4 0. 006 
70.004 
4 0. 002 


0. 000 


Fig.2 Comparison of matching time between 


proposed algorithm and LK optical flow m 

4.2 两 帧 之 间 匹 配 精 度 评 估 
"ur AR PR UESTRAE exon RIT oo AE IE BUG RC R, 
极 约束 : 


GbRm 20 


于 测量 误 


STE, 


上 式 几 乎 不 可 能 成 立 ， 故 求 


ethod 
则 应 该 满足 对 
(28) 
其 最 小 


OpenCV 


特征 点 检测 算法 相 比 时 ， 中 
其 运行 时 间 提 高 了 76.12596, 25 


[zzi 


H 


这 个 精度 结果 也 是 可 以 被 接受 的 。 


IX 1 所 示 ， 


53.295%， 这 得 益 


虑 实际 情况 中 的 实时 性 


| 比 结果 充分 说 明了 
E 。 另 外, 当 与 OpenCV 中 ORB 
然 算 法 精度 降低 了 13.554%， 但 


del 对 极 约束 得 分 
Tab. 1 Score for polar constraints 
得 分 提高 
proposed 28.7961 -- 
LK 光 流 18.7847 53.295% 
ORB 33.3111 -13.55496 
图 4-6 分 别 为 传统 高 斯 牛顿 法 优化 结果 、OpenCV PI 


配 优 
片 为 


化 结果 以 及 本 文 提 吕 
MH 02 easy 序列 中 机 


序 为 匹配 对 信 


Ac. B 


认 初 始 值 旋转 矩阵 为 单位 针 


上 算法 匹配 优化 结果 ， 
FAAR 
4 在 没有 给 定 初始 值 的 ; 


日 邻 两 张 图 片 ， 


展示 选用 的 


Z] 


[A] 


特征 点 


Du 


EWT, 


E 阵 ， 平 移 向 


量 为 零 向 量 。 当 物 


本 


运动 过 快 ， 3 


较 大 ， 误 匹配 现象 较 多 。 对 
初始 值 进 行当 前 帧 


对 的 误 匹 配 ， 


4.3 


VINS-MONO 
在 VI-SLAM 


提出 的 


Hij 端 中 , 


系统 


IER 


改进 


5、6 可 以 看 出 ， 
局 部 邻 域 搜索 匹配 点 ， 
OpenCV 中 光 流 法 仍然 会 出 现 误 
如 编号 : 2，11，20，21 
系统 定位 精度 评估 
本 小 节 本 文 将 


图 像 模糊 程度 较 高 ， 旋 转 位 姿 与 默认 初始 值 偏差 


于 给 定 了 


基本 避免 了 特征 点 
匹配 现象 ， 例 
，22，27，33，39，49 等 。 


点 


光 流 匹配 算法 移植 到 经 典 的 
其 目的 是 进一步 验证 提出 的 优化 方法 
上 的 准确 性 和 稳定 性 。 为 此 ， 将 改进 的 框 


4 Ej VINS-Mono,PL-VIO, Mono-IMU,Stereo-IMU 通过 EVO 


评估 工具 进行 对 比 。 其 9 


y 


加 入 点 线 特 行 


Visual Odometry 榜 单 中 排名 最 靠 前 的 开源 方案 及 改进 算法 。 


H, PL-VIO 是 在 VINS-Mono 前 端 


F 改 进 ; Mono-IMU, Stereo-IMU 是 VINS-Fusion 
在 VINS-Mono 基础 上 加 入 多 传感器 改进 ， 并 且 是 KITTI 


与 文献 [12] 一 样 ， 将 对 齐 之 后 绝对 
Trajectory Error，ATE) 的 均 方 根 误差 (Root Mean Square Error, 


RMSE) 和 误差 均值 作为 评估 


行 20 次 实验 取 均 值 对 比 , 如 


X 2 所 示 , ded 


指标 。 为 防止 结果 的 但 


轨迹 误差 (Absolute 


的 算法 在 EuRoC 
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数据 集 上 , 均 优 于 PL-VIO 框架 , 与 VINS-Fusion 性 能 相近 ， 个 数据 集 外 ， 其 他 序列 都 有 提升 ， 最 为 显著 的 是 
并 且 经 多 次 实验 ，VINS-Fusion Æ V2 03 difficult 数据 集 上 ”V2 03 difficult 序列 ， 具 有 飞行 器 摆动 较 快 ， 图 像 模 糊 程 度 
会 出 现 偏离 真实 轨迹 特别 远 的 情况 。 值 得 注意 的 是 ， 由 于 较 高 等 特点 ， 是 具有 挑战 性 的 数据 集 之 一 ， 其 RMSE 降低 
MH 03 medium 5j V2 01 easy 数据 集 在 两 个 主导 方向 x 和 18.066%， 误 差 均 值 降低 28.902%。 另 外 ， 图 7、8 展示 了 提 
y 的 陀螺 仪 、 加 速度 计 偏 置 较 其 他 数据 集 平均 误差 较 大 ， 不 出 的 算法 与 在 V2 03 difficult 序列 估计 轨迹 与 真实 轨迹 比较 
符合 提出 算法 强 依 赖 于 整个 系统 运动 的 精确 初始 化 这 一 基本 AR; 图 9、10 展示 了 V2 03 difficult 序列 中 提出 的 算法 
假设 ， 所 以 这 两 个 序列 性 能 差 于 VINS-Mono。 更 具体 而 言 ， 与 VINS-Mono 算法 相对 轨迹 误差 的 均 方 根 、 均值 、 标准 差 。 


li 
如 果 没 有 好 的 初始 值 ， 优 化 算法 将 退化 为 无 初始 化 的 高 斯 牛 
除 这 


顿 法 ， 这 不 仅 会 增加 寻 优 时 间 ， 也 会 影响 系统 精确 


FEE 
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图 4 无 初始 值 的 高 斯 和 


Fig.4 Gaussian Newton iterative matching se without initial values 


图 5 OpenCV LK 3k 


TN 


匹配 算法 


下 本 算法 


Fig.5 Opencv LK optical flow matching algorithm 


图 6 提出 的 匹配 算法 


Fig.6 The proposed matching algorithm 


Es 


2 多 种 框架 性 能 


比较 


Tab.2 Performance comparison of multiple frameworks 


通过 这 些 比较 ， 表 明 提 出 的 算法 在 相机 剧烈 抖动 情况 下 ， 重 


序列 提出 的 算法 /(m) VINS-Mono/(m) PL-VIO/(m) Mono-IMU/(m) Stereo-IMU/(m) 

RMSE 均值 RMSE 均值 RMSE ”均值 RMSE 均值 RMSE 均值 

MH 02 easy 0.173 0.139 0.194 0.164 0.186 0.158 0.184 0.152 0.184 0.151 
MH. 03 medium 0.408 0.322 0.402 0.314 0.422 0.334 0.391 0.296 0.468 0.416 
MH. 04 difficult 0.394 0.325 0.402 0.331 0.466 0.395 0.398 0.326 0.520 0.463 
V1 01 easy 0.143 0.119 0.154 0.131 0.145 0.125 0.147 0.125 0.172 0.156 
V1 02 medium 0.306 0.249 0.307 0.250 0.323 0.269 0.307 0.256 0.307 0.254 
V1 03 difficult 0.319 0.278 0.325 0.287 0.374 0.330 0.299 0.261 0.265 0.218 
V2 01 easy 0.127 0.108 0.124 0.106 0.142 0.121 0.123 0.103 0.150 0.131 
V2 02 medium 0.259 0.213 0.262 0.216 0.271 0.226 0.275 0.228 0.260 0.218 
V2 03 difficult 0.322 0.276 0.393 0.346 0.486 0.435 / / 0.404 0.364 
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图 7 提出 的 算法 在 V2 03 difficult 序列 热力 区 
Fig.7 Heat map ofthe proposed algorithm in 
V2 03 difficult sequence 


图 8 Vins-Mono 在 V2 03 difficult 序列 热力 图 
Fig.8 Heat map of the Vins-Mono in V2 03 difficult sequence 


图 9 提出 的 算法 在 V2_03 difficult 序列 表现 
Fig.9 Performance of the proposed algorithm in V2 03 difficult 


sequence 


图 10 VINS-Mono 在 V2 03 difficult JF 
Fig.10 Performance of the VINS-Mono in V2 03 difficult sequence 


列表 现 


5 ”结束 语 
本 文 提出 了 一 种 融合 视觉 


和 IMU 对 图 像 进行 去 除 运 动 
模糊 预 处 理 ， 再 改进 光 流 法 对 特征 点 匹配 的 新 算法 。 该 算法 
充分 利用 IMU 的 运动 信息 , 实现 去 除 图 像 运动 模糊 信息 并 为 
和 迭代 匹配 算法 提供 初始 值 ， 减 小 特征 点 对 误 匹 配 概率 。 该 算 
的 模块 ， 可 以 移植 到 其 他 VI-SLAM 框架 中 。 
司 难 度 层 次 的 数据 集 上 进行 验证 表明 ， 该 算法 
度 都 优 于 OpenCV 库 函 数 中 光 流 匹配 算法 ， 并 
将 提出 的 算法 集成 到 最 先进 的 VIO 框架 VINS-Mono 上 ， 
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在 位 姿 估 计 方 面 提高 了 精度 。 将 来 可 以 对 去 模糊 算法 进一步 
研究 ， 加 入 正则 项 去 除 伪 影 ， 进 一 步 提高 匹配 精度 。 
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